#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_system.h"
#include "esp_log.h"
#include "driver/uart.h"
#include "string.h"

#include "cmd/cmd.h"
#include "Rs485Interface/Rs485Interface.h"
#include "gcode_cmd.h"

#define gcodeAxisX                "X"                       //X轴
#define gcodeAxisY                "Y"                       //Y轴
#define gcodeAxisZ                "Z"                       //Z轴
#define gcodeAxisA                "A"                       //A轴
#define gcodeAxisB                "B"                       //A轴
#define gcodeAxisC                "C"                       //A轴

#define gcodeCfgAll               "$$"                      //查询所有
#define gcodeCfgDir               "$3"                      //步进电机方向
#define gcodeCfgHLimit            "$21"                     //启动硬限位
#define gcodeCfgHoming            "$22"                     //启动归位循环
#define gcodeCfgHomingSpeedH      "$24"                     //归零速度，前段快速移动速度
#define gcodeCfgHomingSpeedL      "$25"                     //归零速度，后段慢速移动速度
#define gcodeCfgTravelResX        "$100"                    //X轴行程分辨率
#define gcodeCfgMaxSpeedX         "$110"                    //X轴最大速度
#define gcodeCfgAccelerationX     "$120"                    //X轴加速度
#define gcodeCfgStrokeX           "$130"                    //X轴最大行程

#define gcodeGoHome               "$H"                      //回原点

#define gcodeMotionMoveFast       "G0"                      //快速移动
#define gcodeMotionMoveCtrl       "G1"                      //控制移动
#define gcodeAbsPos               "G90"                     //绝对定位
#define gcodeRelaPos              "G91"                     //相对定位
#define gcodeDefCurPos            "G92"                     //定义当前位置

#define gcodeMaxAxisNum           10

extern CmdHandle_t CmdHandle;

static const char *TestTag = "gcodeTest";
static const char *gcodeTag = "gcodeLog";

const char* gcodeAxis[AxisNum] = {gcodeAxisX, gcodeAxisY, gcodeAxisZ, gcodeAxisA, gcodeAxisB, gcodeAxisC, NULL, NULL};

uint16_t gcodeQueries(const char *cmd, char *buf);

void set_all(void* n)
{
    Rs485SendData(27, UART_NUM_1, "$$\r\n", strlen("$$\r\n"));
}

void set_pos(void* n)
{
    Rs485SendData(27, UART_NUM_1, "?", strlen("?"));
}

void get_test(void* n)
{
    ESP_LOGI(TestTag,"gcode get test");
}

void test_gcode_task(void *pvParameters) 
{
    MyCMDRegisterIns(&CmdHandle, InsSet, "gcodeall", set_all);
    MyCMDRegisterIns(&CmdHandle, InsSet, "gcodepos", set_pos);
    MyCMDRegisterIns(&CmdHandle, InsGet, "gcodetest", get_test);
    vTaskDelete(NULL);
}

uint16_t gcodeQueries(const char *cmd, char *buf)
{
    sprintf(buf, cmd,"\r\n");
    return strlen(buf);
}

uint16_t gcodeQuerisePos(char *buf)
{
    return gcodeQueries("?", buf);
}

uint16_t gcodeSetParameter(bool P1, int P2, const char *cmd, char *buf)
{
    char _dir[5];
    if ( strncmp(cmd, gcodeCfgDir, strlen(cmd)) == 0 )
    {
        sprintf(buf, "%s%s%s%s", gcodeCfgDir, "=", P1 ? "1" : "0", "\r\n");
        return strlen(buf);
    }
    if ( strncmp(cmd, gcodeCfgHLimit, strlen(cmd)) == 0 )
    {
        sprintf(buf, "%s%s%s%s", gcodeCfgHLimit, "=", P1 ? "1" : "0", "\r\n");
        return strlen(buf);
    }
    if ( strncmp(cmd, gcodeCfgHoming, strlen(cmd)) == 0 )
    {
        sprintf(buf, "%s%s%s%s", gcodeCfgHoming, "=", P1 ? "1" : "0", "\r\n");
        return strlen(buf);
    }
    if ( strncmp(cmd, gcodeCfgHomingSpeedH, strlen(cmd)) == 0 )
    {
        char _dir[5];
        snprintf(_dir, sizeof(_dir), "%d", P2);
        sprintf(buf, "%s%s%s%s", gcodeCfgHomingSpeedH, "=", _dir, "\r\n");
        return strlen(buf);
    }
    if ( strncmp(cmd, gcodeCfgHomingSpeedL, strlen(cmd)) == 0 )
    {
        char _dir[5];
        snprintf(_dir, sizeof(_dir), "%d", P2);
        sprintf(buf, "%s%s%s%s", gcodeCfgHomingSpeedL, "=", _dir, "\r\n");
        return strlen(buf);
    }
    if ( strncmp(cmd, gcodeCfgTravelResX, strlen(cmd)) == 0 )
    {
        char _dir[5];
        snprintf(_dir, sizeof(_dir), "%d", P2);
        sprintf(buf, "%s%s%s%s", gcodeCfgTravelResX, "=", _dir, "\r\n");
        return strlen(buf);
    }
    if ( strncmp(cmd, gcodeCfgMaxSpeedX, strlen(cmd)) == 0 )
    {
        char _dir[5];
        snprintf(_dir, sizeof(_dir), "%d", P2);
        sprintf(buf, "%s%s%s%s", gcodeCfgMaxSpeedX, "=", _dir, "\r\n");
        return strlen(buf);
    }
    if ( strncmp(cmd, gcodeCfgAccelerationX, strlen(cmd)) == 0 )
    {
        char _dir[5];
        snprintf(_dir, sizeof(_dir), "%d", P2);
        sprintf(buf, "%s%s%s%s", gcodeCfgAccelerationX, "=", _dir, "\r\n");
        return strlen(buf);
    }
    if ( strncmp(cmd, gcodeCfgStrokeX, strlen(cmd)) == 0 )
    {
        char _dir[5];
        snprintf(_dir, sizeof(_dir), "%d", P2);
        sprintf(buf, "%s%s%s%s", gcodeCfgStrokeX, "=", _dir, "\r\n");
        return strlen(buf);
    }
    return 0;
}

uint8_t gcodeAnalyzePos(char *data, double *pos)
{
    if (strstr(data, "WPos:") == NULL)
    {
        return 0;
    }
    uint8_t num = 0;
    char *posStart = strstr(data, "<");
    char *posEnd = strstr(data, ">");
    if (posStart != NULL && posEnd != NULL)
    {
        posStart = strnstr(posStart, "WPos:", posEnd - posStart);
        posEnd = strnstr(posStart, "Lim", posEnd - posStart);
        if (posStart != NULL && posEnd != NULL)
        {
            posStart += strlen("WPos:");
            size_t len = posEnd - posStart;
            if (len > 0)
            {

                char result[len];
                strncpy(result, posStart, len);
                do
                {
                    posEnd = strstr(posStart, ",");
                    if (posEnd != NULL)
                    {
                        len = posEnd - posStart;
                        char apos[len];
                        strncpy(apos, posStart, len);
                        // ESP_LOGI("gcode", "%s", apos);
                        pos[num++] = atof(apos);
                        posStart = posEnd + strlen(",");
                    }
                } while (posEnd != NULL);
            }
        }
    }
    return num;
}

void gcodeAnalyzeCfg(char *data, gcodeCfgPara_t *saveData)
{
    char *pos;
    char *pos1;
    char result[10];

    pos = strstr(data, "$");
    if (pos == NULL)
    {
        return;
    }

    pos = strstr(data, gcodeCfgDir);
    if (pos != NULL)
    {
        pos1 = strstr(pos, "\r\n");
        if (pos1 != NULL)
        {
            strncpy(result, pos + strlen(gcodeCfgDir) + strlen("="), (pos1 - pos));
            saveData->dir = atoi(result);
        }
    }

    pos = strstr(data, gcodeCfgHLimit);
    if (pos != NULL)
    {
        pos1 = strstr(pos, "\r\n");
        if (pos1 != NULL)
        {
            strncpy(result, pos + strlen(gcodeCfgHLimit) + strlen("="), (pos1 - pos));
            saveData->hLimit = atoi(result);
        }
    }

    pos = strstr(data, gcodeCfgHoming);
    if (pos != NULL)
    {
        pos1 = strstr(pos, "\r\n");
        if (pos1 != NULL)
        {
            strncpy(result, pos + strlen(gcodeCfgHoming) + strlen("="), (pos1 - pos));
            saveData->homing = atoi(result);
        }
    }

    pos = strstr(data, gcodeCfgHomingSpeedH);
    if (pos != NULL)
    {
        pos1 = strstr(pos, "\r\n");
        if (pos1 != NULL)
        {
            strncpy(result, pos + strlen(gcodeCfgHomingSpeedH) + strlen("="), (pos1 - pos));
            saveData->homingSpeedH = atoi(result);
        }
    }

    pos = strstr(data, gcodeCfgHomingSpeedL);
    if (pos != NULL)
    {
        pos1 = strstr(pos, "\r\n");
        if (pos1 != NULL)
        {
            strncpy(result, pos + strlen(gcodeCfgHomingSpeedL) + strlen("="), (pos1 - pos));
            saveData->homingSpeedL = atoi(result);
        }
    }
        
    pos = strstr(data, gcodeCfgTravelResX);
    if (pos != NULL)
    {
        pos1 = strstr(pos, "\r\n");
        if (pos1 != NULL)
        {
            strncpy(result, pos + strlen(gcodeCfgTravelResX) + strlen("="), (pos1 - pos));
            saveData->travelResX = atoi(result);
        }
    }

    pos = strstr(data, gcodeCfgMaxSpeedX);
    if (pos != NULL)
    {
        pos1 = strstr(pos, "\r\n");
        if (pos1 != NULL)
        {
            strncpy(result, pos + strlen(gcodeCfgMaxSpeedX) + strlen("="), (pos1 - pos));
            saveData->maxSpeedX = atoi(result);
        }
    }

    pos = strstr(data, gcodeCfgAccelerationX);
    if (pos != NULL)
    {
        pos1 = strstr(pos, "\r\n");
        if (pos1 != NULL)
        {
            strncpy(result, pos + strlen(gcodeCfgAccelerationX) + strlen("="), (pos1 - pos));
            saveData->accelerationX = atoi(result);
        }
    }

    pos = strstr(data, gcodeCfgStrokeX);
    if (pos != NULL)
    {
        pos1 = strstr(pos, "\r\n");
        if (pos1 != NULL)
        {
            strncpy(result, pos + strlen(gcodeCfgStrokeX) + strlen("="), (pos1 - pos));
            saveData->strokeX = atoi(result);
        }
    }
    // ESP_LOGI(gcodeTag, "dir is:%d\r\nhLimit is:%d\r\nhoming is:%d\r\nhomingSpeedH is:%d\r\nhomingSpeedL is:%d\r\ntravelResX is:%d\r\nmaxSpeedX is:%d\r\naccelerationX is:%d\r\nstrokeX is:%d\r\n",
    //     saveData->dir, saveData->hLimit, saveData->homing, saveData->homingSpeedH, saveData->homingSpeedL, saveData->travelResX, saveData->maxSpeedX,
    //     saveData->accelerationX, saveData->strokeX);
}

bool gcodeAnalyze(char *data, gcodeCfgPara_t *saveData, double *pos)
{
    bool flag = gcodeAnalyzePos(data, pos);
    gcodeAnalyzeCfg(data, saveData);

    return flag;
}

uint16_t gcodeMoveFast(MotorAxis_t MotorAxis, int distance, char *buf)
{
    if (MotorAxis < AxisNum)
    {
        if(gcodeAxis[MotorAxis] != NULL)
        {
            sprintf(buf, "%s%s%s%d%s", gcodeMotionMoveFast, gcodeRelaPos, gcodeAxis[MotorAxis], distance, "\r\n");
            return strlen(buf);
        }
        return 0;
    }
    return 0;
}

uint16_t gcodeMoveCtrl(MotorAxis_t MotorAxis, int distance, char *buf)
{
    if (MotorAxis < AxisNum)
    {
        if(gcodeAxis[MotorAxis] != NULL)
        {
            sprintf(buf, "%s%s%s%d%s", gcodeMotionMoveCtrl, gcodeRelaPos, gcodeAxis[MotorAxis], distance, "\r\n");
            return strlen(buf);
        }
        return 0;
    }
    return 0;
}

uint16_t gcodeGoHomeAll(char *buf)
{
    sprintf(buf, "%s%s", gcodeGoHome, "\r\n");
    return strlen(buf);
}

uint16_t gcodeGoHomeAxis(MotorAxis_t MotorAxis, char *buf)
{
    if (MotorAxis < AxisNum)
    {
        if(gcodeAxis[MotorAxis] != NULL)
        {
            sprintf(buf, "%s%s%s", gcodeGoHome, gcodeAxis[MotorAxis], "\r\n");
            return strlen(buf);
        }
        return 0;
    }
    return 0;
}

